using System;
using System.Collections.Generic;
using System.Text;

namespace GameLib.Utils.Pathing
{
    public class PathFinder<NodeType>
    {
	    /// <summary>
	    /// A heuristic is a function that associates a value with a node to gauge it considering the node to reach.
	    /// </summary>
        public delegate float Heuristic(NodeType NodeToEvaluate, NodeType TargetNode);

        public delegate bool GoalEvaluator(NodeType nodeToEval);


		//Graph _Graph;
        PriorityQueueB<Track<NodeType>> OpenList = new PriorityQueueB<Track<NodeType>>();

        Dictionary<NodeType, Track<NodeType>> Tracks = new Dictionary<NodeType, Track<NodeType>>();

        //SortableList _Open = new SortableList();
        //SortableList _Closed = new SortableList();
		//Track _LeafToGoBackUp;
		int _NbIterations = -1;

		//SortableList.Equality SameNodesReached = new SortableList.Equality( Track.SameEndNode );

        /// <summary>
		/// Gets/Sets the heuristic that AStar will use.
		/// It must be homogeneous to arc's cost.
		/// </summary>

        private GoalEvaluator m_GoalEval;

        public GoalEvaluator GoalEval
        {
            get { return m_GoalEval; }
            set 
            {
                if (value == null)
                    throw new ArgumentNullException();

                m_GoalEval = value; 
            }
        }

		public IPathNodeSemantics<NodeType> PathNodeSemantics
		{
			get;
			private set;
		}

        public bool DefaultGoalEvaluator(NodeType nodeToEval)
        {
			return PathNodeSemantics.Equals(nodeToEval, Goal);
        }


		/// <summary>
		/// AStar Constructor.
		/// </summary>
		/// <param name="G">The graph on which AStar will perform the search.</param>
		public PathFinder(IPathNodeSemantics<NodeType> pathNodeSemantics)
		{
			PathNodeSemantics = pathNodeSemantics;
            m_GoalEval = DefaultGoalEvaluator;
			//_Graph = G;
			//_Open = new SortableList();
			//_Closed = new SortableList();
			//ChoosenHeuristic = EuclidianHeuristic;
			//DijkstraHeuristicBalance = 0.5;
		}

		/// <summary>
		/// Searches for the best path to reach the specified EndNode from the specified StartNode.
		/// </summary>
		/// <exception cref="ArgumentNullException">StartNode and EndNode cannot be null.</exception>
		/// <param name="StartNode">The node from which the path must start.</param>
		/// <param name="EndNode">The node to which the path must end.</param>
		/// <returns>'true' if succeeded / 'false' if failed.</returns>
		public bool SearchPath(NodeType startNode, NodeType endNode)
		{
            Initialize(startNode, endNode);

            Track<NodeType> startTrack = new Track<NodeType>(startNode);
            Tracks[startNode] = startTrack;

            if (GoalEval(startNode))
            {
                FinishPath(startTrack);
                return true;
            }

            startTrack.State = TrackState.Closed;
            Propagate(startTrack);

			while ( NextStep() ) {}
            return IsPathFound;
		}
/*
		/// <summary>
		/// Use for debug in 'step by step' mode only.
		/// Returns all the tracks found in the 'Open' list of the algorithm at a given time.
		/// A track is a list of the nodes visited to come to the current node.
		/// </summary>
		public Node[][] Open
		{
			get
			{
				Node[][] NodesList = new Node[_Open.Count][];
				for ( int i=0; i<_Open.Count; i++ ) NodesList[i] = GoBackUpNodes((Track)_Open[i]);
				return NodesList;
			}
		}

		/// <summary>
		/// Use for debug in a 'step by step' mode only.
		/// Returns all the tracks found in the 'Closed' list of the algorithm at a given time.
		/// A track is a list of the nodes visited to come to the current node.
		/// </summary>
		public Node[][] Closed
		{
			get
			{
				Node[][] NodesList = new Node[_Closed.Count][];
				for ( int i=0; i<_Closed.Count; i++ ) NodesList[i] = GoBackUpNodes((Track)_Closed[i]);
				return NodesList;
			}
		}*/

        private NodeType Goal;

		/// <summary>
		/// Use for a 'step by step' search only. This method is alternate to SearchPath.
		/// Initializes AStar before performing search steps manually with NextStep.
		/// </summary>
		/// <exception cref="ArgumentNullException">StartNode and EndNode cannot be null.</exception>
		/// <param name="StartNode">The node from which the path must start.</param>
		/// <param name="EndNode">The node to which the path must end.</param>
        protected void Initialize(NodeType startNode, NodeType endNode)
		{
            if (startNode == null || endNode == null) throw new ArgumentNullException();

            OpenList.Clear();
            Tracks.Clear();
            m_FoundPath.Clear();

            Goal = endNode;

			_NbIterations = 0;
		}

		/// <summary>
		/// Use for a 'step by step' search only. This method is alternate to SearchPath.
		/// The algorithm must have been initialize before.
		/// </summary>
		/// <exception cref="InvalidOperationException">You must initialize AStar before using NextStep().</exception>
		/// <returns>'true' unless the search ended.</returns>
		protected bool NextStep()
		{

            if (OpenList.Count == 0)
            {
                return false;
            }

			_NbIterations++;

            Track<NodeType> bestTrack = OpenList.Pop();

            if (GoalEval(bestTrack.EndNode)) // TODO: goal evaluation
			{
                FinishPath(bestTrack);
                OpenList.Clear();
			}
			else
			{
                if (bestTrack.State != TrackState.Closed)
                {
                    if (PathNodeSemantics.CanEnter(bestTrack.EndNode))
                        Propagate(bestTrack);
                    bestTrack.State = TrackState.Closed;
                }
			}
            return OpenList.Count > 0;
		}

        protected void AddToOpen(Track<NodeType> track)
        {
            track.State = TrackState.Opened;
            OpenList.Push(track);
        }


		private void Propagate(Track<NodeType> curTrack)
		{

			foreach (NodeType node in PathNodeSemantics.GetNeighbours(curTrack.EndNode))
			{
				float cost = PathNodeSemantics.GetPathCost(curTrack.EndNode, node);


				if ( cost >= 0.0f )
				{
                    float moveCost = curTrack.MoveCost + cost;
                    Track<NodeType> track;
                    
                    if (Tracks.TryGetValue(node, out track))
                    {
                        if (moveCost >= track.MoveCost)
                            continue;

                        // if in open list, leave it there, will ignore it once it becomes closed
                        track.PrevTrack = curTrack;
                    }
                    else
                    {
                        track = new Track<NodeType>(curTrack, node);
                        Tracks[node] = track;
                    }

                    track.MoveCost = moveCost;
                    track.HeuristicCost = PathNodeSemantics.GetHeuristicCost(curTrack.EndNode, track.EndNode, Goal);

                    AddToOpen(track);
				}
			}
		}


        protected Stack<NodeType> m_FoundPath = new Stack<NodeType>();

        public IEnumerable<NodeType> FoundPath
        {
            get { return m_FoundPath; }
        }

		public IEnumerable<NodeType> FoundPathNoStart
		{
			get
			{
				bool isFirst = true;

				foreach (NodeType node in FoundPath)
				{
					if (isFirst)
					{
						isFirst = false;
					}
					else
					{
						yield return node;
					}
				}
			}
		}

        public bool IsPathFound
        {
            get { return m_FoundPath.Count > 0; }
        }
        
        private void FinishPath(Track<NodeType> endTrack)
        {
            m_FoundPath.Clear();

            Track<NodeType> track = endTrack;
            
            while (track != null)
            {
                m_FoundPath.Push(track.EndNode);
                track = track.PrevTrack;
            }
            //Console.WriteLine("finished pathing, taking iter {0}", _NbIterations);
        }



        /*
		/// <summary>
		/// To know if the search has been initialized.
		/// </summary>
		public bool Initialized { get { return _NbIterations>=0; } }

		/// <summary>
		/// To know if the search has been started.
		/// </summary>
		public bool SearchStarted { get { return _NbIterations>0; } }

		/// <summary>
		/// To know if the search has ended.
		/// </summary>
		public bool SearchEnded { get { return SearchStarted && _Open.Count==0; } }

		/// <summary>
		/// To know if a path has been found.
		/// </summary>
		public bool PathFound { get { return _LeafToGoBackUp!=null; } }

		/// <summary>
		/// Use for a 'step by step' search only.
		/// Gets the number of the current step.
		/// -1 if the search has not been initialized.
		/// 0 if it has not been started.
		/// </summary>
		public int StepCounter { get { return _NbIterations; } }

		private void CheckSearchHasEnded()
		{
			if ( !SearchEnded ) throw new InvalidOperationException("You cannot get a result unless the search has ended.");
		}

		/// <summary>
		/// Returns information on the result.
		/// </summary>
		/// <param name="NbArcsOfPath">The number of arcs in the result path / -1 if no result.</param>
		/// <param name="CostOfPath">The cost of the result path / -1 if no result.</param>
		/// <returns>'true' if the search succeeded / 'false' if it failed.</returns>
		public bool ResultInformation(out int NbArcsOfPath, out double CostOfPath)
		{
			CheckSearchHasEnded();
			if ( !PathFound )
			{
				NbArcsOfPath = -1;
				CostOfPath = -1;
				return false;
			}
			else
			{
				NbArcsOfPath = _LeafToGoBackUp.NbArcsVisited;
				CostOfPath = _LeafToGoBackUp.Cost;
				return true;
			}
		}

		/// <summary>
		/// Gets the array of nodes representing the found path.
		/// </summary>
		/// <exception cref="InvalidOperationException">You cannot get a result unless the search has ended.</exception>
		public Node[] PathByNodes
		{
			get 
			{
				CheckSearchHasEnded();
				if ( !PathFound ) return null;
				return GoBackUpNodes(_LeafToGoBackUp);
			}
		}

		private Node[] GoBackUpNodes(Track T)
		{
			int Nb = T.NbArcsVisited;
			Node[] Path = new Node[Nb+1];
			for (int i=Nb; i>=0; i--, T=T.Queue)
			Path[i] = T.EndNode;
			return Path;
		}

		/// <summary>
		/// Gets the array of arcs representing the found path.
		/// </summary>
		/// <exception cref="InvalidOperationException">You cannot get a result unless the search has ended.</exception>
		public Arc[] PathByArcs
		{
			get 
			{
				CheckSearchHasEnded();
				if ( !PathFound ) return null;
				int Nb = _LeafToGoBackUp.NbArcsVisited;
				Arc[] Path = new Arc[Nb];
				Track Cur=_LeafToGoBackUp;
				for (int i=Nb-1; i>=0; i--, Cur = Cur.Queue)
					Path[i] = Cur.Queue.EndNode.ArcGoingTo(Cur.EndNode);
				return Path;
			}
		}

		/// <summary>
		/// Gets the array of points representing the found path.
		/// </summary>
		/// <exception cref="InvalidOperationException">You cannot get a result unless the search has ended.</exception>
		public Point3D[] PathByCoordinates
		{
			get 
			{
				CheckSearchHasEnded();
				if ( !PathFound ) return null;
				int Nb = _LeafToGoBackUp.NbArcsVisited;
				Point3D[] Path = new Point3D[Nb+1];
				Track Cur=_LeafToGoBackUp;
				for (int i=Nb; i>=0; i--, Cur = Cur.Queue)
					Path[i] = Cur.EndNode.Position;
				return Path;
			}
		}*/

    }
}
